'Allows an eRacer robot to be controlled using a Lego Mindstorms remote

'Includes
#include <eracer.h>
#include <lego.h>

'Inter-layer communication
#define Beep Accessories.0
#define LED_G Accessories.1
#define LED_R Accessories.2
#define LED_T Accessories.3

#define R_Stop 0
#define R_Forward 1
#define R_Reverse 2
#define All_Off 255

'Settings for programs
LINE = 50
#define BackTime 75
#define SpinTime 50

'Initialisation subs (Note: InitBot and InitLego called using #startup)
InitRemote

'Main Routine
Main:
 Receive
 Process
 RunProgram
 Act
goto Main

sub InitRemote
 Accessories = 0
 LeftMotor = 0
 RightMotor = 0
 Program = 0
end sub

sub Receive
 'Initialise
 BadSignal = FALSE
 RemoteH = 0
 RemoteL = 0
 if RecALow then BadSignal = TRUE: exit sub
 
 'Read Header
 SerReceive(1, LegoTemp)
 If LegoTemp = 0x55 then SerReceive (1, LegoTemp)
 if LegoTemp <> 0xff then BadSignal = TRUE: exit sub
 SerReceive(1, LegoTemp)
 'Read Command
 SerReceive(1, LegoTemp)
 if LegoTemp <> 0xd2 then BadSignal = TRUE: Wait 15 ms: exit sub
 SerReceive(1, LegoTemp)
 'Read Button status
 SerReceive(1, ButtonL)
 SerReceive(1, LegoTemp)
 SerReceive(1, ButtonH)
 SerReceive(1, LegoTemp)
 'Read Checksum
 SerReceive(1, Checksum)
 SerReceive(1, LegoTemp)

 'Verify Checksum
 LegoTemp = 0xd2 + ButtonL + ButtonH
 if LegoTemp <> Checksum then BadSignal = TRUE

 'Remote sends each command 3 times, so wait for the other two sends to complete
 Wait 10 ms 
end sub

sub Process
 if BadSignal = TRUE then exit sub

 LeftMotor = R_Stop
 RightMotor = R_Stop
 
 'Program selection buttons
 if ButtonH = 0 then
  if ButtonL = 0x02 then Program = 1
  if ButtonL = 0x04 then Program = 2
  if ButtonL = 0x08 then Program = 3
  if ButtonL = 0x10 then Program = 4
  if ButtonL = 0x20 then Program = 5
 end if
 
 if ButtonL.0 ON then RightMotor = R_Reverse
 if ButtonL = 0x40 then LeftMotor = R_Stop: RightMotor = R_Stop: Accessories = All_Off: Program = 0: exit sub
 if ButtonL = 0x80 then set beep on: exit sub  

 if ButtonH = 0x01 then set LED_G ON: exit sub
 if ButtonH = 0x02 then set LED_R ON: exit sub
 if ButtonH = 0x04 then set LED_T ON: exit sub
 if ButtonH.3 ON then LeftMotor = R_Forward
 if ButtonH.5 ON then RightMotor = R_Forward
 if ButtonH.6 ON then LeftMotor = R_Reverse
end sub

sub RunProgram
 if Program = 0 then exit sub
 
 'Line follow program
 if Program = 1 then
  SET GREEN ON
  SET RED ON
  Temp = ReadAD (Analog_LDR_Left)
  IF Temp < LINE THEN TurnLeft
  IF Temp > LINE THEN TurnRight
  exit sub 
 end if
 
 'Object avoidance program
 if Program = 2 then
  PWMOn
  Wait 3 10us
  Forward
  if IR_In on then
   PWMOff
   Wait 3 10us
   exit sub
  end if
  if IR_In off then
   PWMOff
   Reverse
   Wait BackTime 10ms
   SpinLeft
   Wait SpinTime 10ms
   Forward
  end if  
 end if

 'Line follow recalibrate program
 if Program = 5 then
  Program = 0
  set green on
  wait 1 ms
  LINE = ReadAD(Analog_LDR_Left)
  wait 1 ms
  set green off
 end if

end sub

sub Act

 'Turn all accessories off?
 if Accessories = All_Off then
  set green off
  set red off
  set TL_LED off
  set TR_LED off
  set buzzer off
  Accessories = 0
 end if

 'Toggle LEDs
 if LED_G on then
  Set LED_G off 
  if green on then set green off: goto GreenDone
  set green on
 end if
 GreenDone:
 if LED_R on then
  Set LED_R off 
  if red on then set red off: goto RedDone
  set red on
 end if
 RedDone:
 if LED_T on then
  Set LED_T off 
  if TL_LED on then set TL_LED off: set TR_LED off: goto TopDone
  set TL_LED on
  set TR_LED on
 end if
 TopDone:

 'Buzzer
  if Beep on then
   set Beep off
   set buzzer on
   wait 10 10ms
   set buzzer off
  end if

 'Control Motors (Don't turn off if a program is running)
  if Program <> 0 then
   if LeftMotor = R_Stop then exit sub
   if RightMotor = R_Stop then exit sub  
  end if
  if LeftMotor = R_Stop then set LF off: set LR off
  if LeftMotor = R_Forward then set LF on: set LR off
  if LeftMotor = R_Reverse then set LF off: set LR on
  if RightMotor = R_Stop then set RF off: set RR off
  if RightMotor = R_Forward then set RF on: set RR off
  if RightMotor = R_Reverse then set RF off: set RR on
end sub